#include "imgproc.h"

struct DIV
{
    int Left;
    int Right;
};

struct DIV edge[MT9V03X_H];                      // 边缘线 // 255代表无效数据
uint8 midLine[MT9V03X_H];                        // 中线位置数组
uint8 threshold_value = 0;                       // 大津法输出的阈值
uint8 mt9v03x_image_BandW[MT9V03X_H][MT9V03X_W]; // 图像转存数组

uint8_t clip(uint8_t value, uint8_t min, uint8_t max)
{
    return (value < min) ? min : (value > max) ? max
                                               : value;
}
uint8_t Otsu_threshold_Add = 4;
uint8_t image_thres = 100, image_thres_min = 20;

/************************************
 * 函数名：Get_Otsu_Threshold(uint8_t mt9v03x_image_dvp[MT9V03X_H][MT9V03X_W])
 * 函数功能：大津法（二值化）
 ************************************/

uint8_t get_Otsu_Threshold(uint8_t mt9v03x_image_dvp[MT9V03X_H][MT9V03X_W])
{
    int16_t i, j;
    uint32_t Amount = 0;
    uint32_t PixelBack = 0;
    uint32_t PixelIntegralBack = 0;
    uint32_t PixelIntegral = 0;
    int32_t PixelIntegralFore = 0;
    int32_t PixelFore = 0;
    float OmegaBack, OmegaFore, MicroBack, MicroFore, SigmaB, Sigma;
    int16_t MinValue, MaxValue;
    int16_t Threshold = 0;
    uint8_t HistoGram[256];

    memset(HistoGram, 0, sizeof(HistoGram));

    // 优化采样方式：隔行全列采样
    for (j = 0; j < MT9V03X_H; j += 2)
    {
        for (i = 0; i < MT9V03X_W; i += 2) // 列方向全采样
        {
            HistoGram[mt9v03x_image_dvp[j][i]]++;
        }
    }

    // 修正最小/最大灰度值计算
    for (MinValue = 0; MinValue < 256 && HistoGram[MinValue] == 0; MinValue++)
        ;
    for (MaxValue = 255; MaxValue > MinValue && HistoGram[MaxValue] == 0; MaxValue--)
        ;

    if (MaxValue == MinValue)
        return (uint8_t)MaxValue;
    if (MinValue + 1 == MaxValue)
        return (uint8_t)MinValue;

    for (j = MinValue; j <= MaxValue; j++)
        Amount += HistoGram[j];

    PixelIntegral = 0;
    for (j = MinValue; j <= MaxValue; j++)
    {
        PixelIntegral += HistoGram[j] * j;
    }

    SigmaB = -1;
    for (j = MinValue; j <= MaxValue; j++) // 包含MaxValue
    {
        PixelBack += HistoGram[j];
        PixelFore = Amount - PixelBack;
        if (PixelFore == 0)
            break;

        OmegaBack = (float)PixelBack / Amount;
        OmegaFore = (float)PixelFore / Amount;
        PixelIntegralBack += HistoGram[j] * j;
        PixelIntegralFore = PixelIntegral - PixelIntegralBack;

        MicroBack = (float)PixelIntegralBack / PixelBack;
        MicroFore = (float)PixelIntegralFore / PixelFore;
        Sigma = OmegaBack * OmegaFore * (MicroBack - MicroFore) * (MicroBack - MicroFore);

        if (Sigma > SigmaB)
        {
            SigmaB = Sigma;
            Threshold = j;
        }
    }

    // 动态调整增量：当阈值较高时减少增量
    uint8_t dynamic_add = (Threshold > 150) ? 2 : Otsu_threshold_Add;

    // 限制阈值上限避免过曝
    return clip(Threshold + dynamic_add, image_thres_min, 200);
}
/**
 * @brief 初始化
 * @return void
 * @note process每次运行前必须运行一次
 **/
void InitData()
{
    int pin;
    for (pin = 0; pin < max(MT9V03X_H, MT9V03X_W); pin++)
    {
        edge[pin].Right = 255;
        edge[pin].Left = 255;

        midLine[pin] = 0;
    }
}
/**
 * @brief 从附近点搜索边界线
 * @return 结束时边界线位置 255为找不到
 * @private
 **/
uint8_t search_Line_From_NearPoint(uint8_t row)
{
    // todo 判断线宽 丢线需要补赛道

    uint8_t colMinLeft;
    uint8_t colMaxLeft;
    uint8_t colMinRight;
    uint8_t colMaxRight;
    bool find = false;
    for (; row < MT9V03X_H - 40; row++)
    {
        colMinLeft = edge[row].Left - 10;
        colMaxLeft = edge[row].Left + 10;
        colMinRight = edge[row].Right - 10;
        colMaxRight = edge[row].Right + 10;
        clip(colMinLeft, 5, MT9V03X_W - 55);
        clip(colMaxLeft, 5, MT9V03X_W - 55);
        clip(colMinRight, 5, MT9V03X_W - 55);
        clip(colMaxRight, 5, MT9V03X_W - 55);
        for (uint8_t col = colMinLeft; col <= colMaxLeft; col++)
        {
            // 左巡边线
            if (mt9v03x_image_BandW[row][col] == BLACK_IMG && mt9v03x_image_BandW[row][col + 1] == BLACK_IMG)
            {
                if (mt9v03x_image_BandW[row][col + 2] == WHITE_IMG && mt9v03x_image_BandW[row][col + 3] == WHITE_IMG)
                {
                    edge[row].Left = col + 1;
                    find = true;
                    break;
                }
            }
            // 右巡边线
            for (uint8_t col = colMaxRight; col >= colMinRight; col--)
            {
                if (mt9v03x_image_BandW[row][col] == BLACK_IMG && mt9v03x_image_BandW[row][col - 1] == BLACK_IMG)
                {
                    if (mt9v03x_image_BandW[row][col - 2] == WHITE_IMG && mt9v03x_image_BandW[row][col - 3] == WHITE_IMG)
                    {
                        edge[row].Left = col - 1;
                        find = true;
                        break;
                    }
                }
            }
        }
    }
    if (find == false)
    {
        return 255;
    }

    return row;
}
/**
 * @brief 搜索单行边界
 * @return 1为找到 0为找不到
 * @private
 **/
bool search_Single_Row(uint8 row)
{
    bool out_flag = 0;

    for (uint8 col = 5; edge[row].Left == 254 && col < MT9V03X_W - 5; col++)
    {
        //====================================================================
        // 找左边界
        if (mt9v03x_image_BandW[row][col] == BLACK_IMG && mt9v03x_image_BandW[row][col + 1] == BLACK_IMG)
        {
            if (mt9v03x_image_BandW[row][col + 2] == WHITE_IMG && mt9v03x_image_BandW[row][col + 3] == WHITE_IMG)
            {
                // edge[row].Left = row;
                edge[row].Left = col;
                out_flag = 1;
            }
        }
        //====================================================================
        // 找右边界
        if (mt9v03x_image_BandW[row][col] == BLACK_IMG && mt9v03x_image_BandW[row][col - 1] == BLACK_IMG)
        {
            if (mt9v03x_image_BandW[row][col - 2] == WHITE_IMG && mt9v03x_image_BandW[row][col - 3] == WHITE_IMG)
            {
                // edge[row].Right = row;
                edge[row].Right = col;
                out_flag = 1;
            }
        }
        if (out_flag == 1)
        {
            return 1;
        }
    }
    return 0;
}

/**
 * @brief 找边线
 * @pre 每次process必须运行一次InitData()
 *
 * @return void 边线保存在struct DIV left, right; // 左右边线
 * @
 **/
void find_Edges()
{

    // 从下到上扫开始线 范围：高度：5 到 H-40 宽度：5 到 W-55
    for (uint8 i = 5; i < MT9V03X_H - 40; i++)
    { // 没有起点就做全扫描
        if (search_Single_Row(i) == 1)
        {
            // 找到起始点，开始寻找周围点
            i = search_Line_From_NearPoint(i);
        }
        // 没找到
    }
}


/**
 * @brief 计算中线位置
 *
 * This function processes the relevant image or sensor data to determine the central line,
 * which can be used for tasks such as line following, feature extraction, or navigation.
 * The specific algorithm and data sources depend on the implementation details within the function.
 *
 * @note Ensure that the necessary input data is prepared before calling this function.
 */
void calc_mid_line()
{

    // 遍历两个leaf 和right 数组 计算得出中线，要考虑丢线情况
    for (uint8 row = 0; row <= MT9V03X_H; row++)
    {
        // 如果其中一边缺失
        if (edge[row].Left == 255 || edge[row].Right == 255)
        {

            // todo 如果缺少一边，需要脑补线
            continue;
        }
        // 如果线距离太大
        if (edge[row].Left + edge[row].Right > MAXWIDTH)
        {
            continue;
        }
        midLine[row] = (edge[row].Left + edge[row].Right) / 2;
        // 更新中线
        // mt9v03x_image_BandW[i][mid_col] = MID_IMG;
    }
}

/************************************
 * 函数名：show_image(void)
 * 函数功能：图像显示
 * 备注：此函数需在图像处理函数后调用
 ************************************/
void show_image(void)
{
    tft180_show_gray_image(0, 0, mt9v03x_image[0], image_w, image_h, 94, 60, threshold_value);
}

/************************************
 * 函数名：image_process(void)
 * 函数功能：图像处理      time=3.5s
 ************************************/
void image_process(void)
{
    memcpy(mt9v03x_image_BandW, mt9v03x_image, sizeof(mt9v03x_image));
    threshold_value = get_Otsu_Threshold(mt9v03x_image_BandW);
    // 二值化
    for (uint8 i = 0; i < MT9V03X_H; i++)
    {
        for (uint8 j = 0; j < MT9V03X_W; j++)
        {
            if (mt9v03x_image_BandW[i][j] >= threshold_value)
            {
                mt9v03x_image_BandW[i][j] = WHITE_IMG;
            }
            else
            {
                mt9v03x_image_BandW[i][j] = BLACK_IMG;
            }
        }
    }
    InitData();
    find_Edges();
    calc_mid_line();

    // show_image();
}
